
// import oimo.collision.broadphase.*;
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.*;
// import oimo.dynamics.constraint.joint.*;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.M;

import { Contact } from "./Contact";
import { M } from "../common/M";
import { RigidBody } from "./rigidbody/RigidBody";
import { ContactLink } from "./ContactLink";
import { Shape } from "./rigidbody/Shape";
import { RigidBodyType } from "./rigidbody/RigidBodyType";
import { Aabb } from "../collision/geometry/Aabb";
import { BroadPhase } from "../collision/broadphase/BroadPhase";
import { ProxyPair } from "../collision/broadphase/ProxyPair";
import { CollisionMatrix } from "../collision/narrowphase/CollisionMatrix";
import { JointLink } from "./constraint/joint/JointLink";

/**
 * The manager of the contacts in the physics world. A contact of two
 * shapes is created when the AABBs of them begin overlapping, and
 * is destroyed when they end overlapping.
 */
export class ContactManager {
	_numContacts: number;
	_contactList: Contact;
	_contactListLast: Contact;
	_contactPool: Contact;

	_broadPhase: BroadPhase;
	_collisionMatrix: CollisionMatrix;


	constructor(broadPhase: BroadPhase) {
		this._broadPhase = broadPhase;
		this._collisionMatrix = new CollisionMatrix();
		this._numContacts = 0;
	}

	// --- ---

	public createContacts(): void {
		var pp: ProxyPair = this._broadPhase._proxyPairList;
		while (pp != null) {
			var s1: Shape;
			var s2: Shape;
			M.assert(pp._p1._id != pp._p2._id);
			if (pp._p1._id < pp._p2._id) {
				s1 = pp._p1.userData;
				s2 = pp._p2.userData;
			} else {
				s1 = pp._p2.userData;
				s2 = pp._p1.userData;
			}
			// collision filtering
			if (!this.shouldCollide(s1, s2)) {
				break;
			}

			// search for the same contact
			var b1: RigidBody = s1._rigidBody;
			var b2: RigidBody = s2._rigidBody;
			var n1: number = b1._numContactLinks;
			var n2: number = b2._numContactLinks;
			var l: ContactLink;
			// select shorter linked list
			if (n1 < n2) {
				l = b1._contactLinkList;
			} else {
				l = b2._contactLinkList;
			}
			var id1: number = s1._id;
			var id2: number = s2._id;
			var found: boolean = false;
			while (l != null) {
				var c: Contact = l._contact;
				if (c._s1._id == id1 && c._s2._id == id2) {
					// the same contact found
					c._latest = true;
					found = true;
					break;
				}
				l = l._next;
			}

			// if not found, create a new contact
			if (!found) {
				// trying to pick an object up from the pool
				var c: Contact;
				if (this._contactPool == null) {
					c = new Contact();
				} else {
					c = this._contactPool;
					this._contactPool = this._contactPool._next;
					c._next = null;
				}
				if (this._contactList == null) {
					this._contactList = c;
					this._contactListLast = c;
				} else {
					this._contactListLast._next = c;
					c._prev = this._contactListLast;
					this._contactListLast = c;
				}
				c._latest = true;
				c._attach(s1, s2, this._collisionMatrix.getDetector(s1._geom._type, s2._geom._type));
				this._numContacts++;
			}
			pp = pp._next;
		}

	}

	public destroyOutdatedContacts(): void {
		// whether the broadphase returns only new overlapping pairs
		var incremental: boolean = this._broadPhase._incremental;

		var c: Contact = this._contactList;
		while (c != null) {
			if (c._latest) {
				// the contact is overlapping, make it old for the next step
				c._latest = false;
				c._shouldBeSkipped = false;
				break;
			}
			if (!incremental) {
				// the pair is separated, because the broad-phase algorithm collects
				// all the overlapping pairs and they are marked as latest
				this._destroyContact(c);
				break;
			}

			var s1: Shape = c._s1;
			var s2: Shape = c._s2;
			var r1: RigidBody = s1._rigidBody;
			var r2: RigidBody = s2._rigidBody;
			var active1: boolean = !r1._sleeping && r1._type != RigidBodyType.STATIC;
			var active2: boolean = !r2._sleeping && r2._type != RigidBodyType.STATIC;
			if (!active1 && !active2) {
				// skip the pair if both rigid bodies are inactive
				c._shouldBeSkipped = true;
				break;
			}
			var aabb1: Aabb = s1._aabb;
			var aabb2: Aabb = s2._aabb;
			if (!this._broadPhase.isOverlapping(s1._proxy, s2._proxy) || !this.shouldCollide(s1, s2)) {
				// the proxy pair is separated or shouldn't collide
				this._destroyContact(c);
				break;
			}
			// the proxies are overlapping, but AABBs might be separated
			var aabbOverlapping: boolean = M.aabb_overlap(aabb1._min, aabb1._max, aabb2._min, aabb2._max);
			// needs narrow-phase collision detection if AABBs are overlapping
			c._shouldBeSkipped = !aabbOverlapping;
			c = c._next;
		}
	}

	public shouldCollide(s1: Shape, s2: Shape): boolean {
		var r1: RigidBody = s1._rigidBody;
		var r2: RigidBody = s2._rigidBody;

		if (r1 == r2) {
			// they have the same parent
			return false;
		}

		if (r1._type != RigidBodyType.DYNAMIC && r2._type != RigidBodyType.DYNAMIC) {
			// none of the two bodies are dynamic
			return false;
		}

		// collision filtering
		if (
			(s1._collisionGroup & s2._collisionMask) == 0 ||
			(s2._collisionGroup & s1._collisionMask) == 0
		) {
			return false;
		}

		// search for joints the two bodies are connected to
		var jl: JointLink;
		var other: RigidBody;
		if (r1._numJointLinks < r2._numJointLinks) {
			jl = r1._jointLinkList;
			other = r2;
		} else {
			jl = r2._jointLinkList;
			other = r1;
		}

		while (jl != null) {
			if (jl._other == other && !jl._joint._allowCollision) {
				// collisions between the two bodies are disabled
				return false;
			}
			jl = jl._next;
		}

		return true;
	}

	// --- internal ---

	public _updateContacts(): void {
		this._broadPhase.collectPairs();
		this.createContacts();
		this.destroyOutdatedContacts();
	}

	// send postSolve events
	public _postSolve(): void {
		var c: Contact = this._contactList;
		while (c != null) {
			if (c._touching) {
				c._postSolve();
			}
			c = c._next;
		}
	}


	public _updateManifolds(): void {
		var c: Contact = this._contactList;

		while (c != null) {

			if (!c._shouldBeSkipped) {
				c._updateManifold();
			}

			c = c._next;
		}

	}


	public _destroyContact(contact: Contact): void {

		let prev = contact._prev;
		let next = contact._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (contact == this._contactList) {
			this._contactList = next;
		}
		if (contact == this._contactListLast) {
			this._contactListLast = prev;
		}
		contact._prev = null;
		contact._next = null;

		contact._detach();

		// put it into the pool
		contact._next = this._contactPool;
		this._contactPool = contact;

		this._numContacts--;
	}

	// --- public ---

	/**
	 * Returns the number of the contacts in the world.
	 */
	public getNumContacts(): number {
		return this._numContacts;
	}

	/**
	 * Returns the linked list of the contacts in the world.
	 */
	public getContactList(): Contact {
		return this._contactList;
	}

}
